function [like] = likelihood(p1,p2,d,dSigma)
    % likelihood function, given distance d(with sigma of dSigma)and p2
    % It isn't normalized
    like =  exp(-1/(2*dSigma^2)*(norm(p1-p2)-d)^2);
end